
#include "PhysicsEngine.h"
#include "PhysicsLocale.h"

using namespace std;
using namespace boost;
using namespace Tool;

namespace Physics
{
	PhysicsEngine::PhysicsEngine() 
	{
		LOGDEBUG("init Physics Engine");		
		m_updatetime = 0.5f; 
		m_nextObjectIdAvailable = 0;
		m_nextLocaleIdAvailable = 0;
		
		if (!InitialiseAgeia())
		{
			LOGERROR("Could not initialise Ageia Physics!\nPlease check you have Ageia runtime installed.");
			ABORT_APP;
		}
	}

	PhysicsEngine::~PhysicsEngine()
	{	
		cleanup();
	}

	bool PhysicsEngine::InitialiseAgeia()
	{
		m_PhysicsSDK = NxCreatePhysicsSDK(NX_PHYSICS_SDK_VERSION);
		if (!m_PhysicsSDK)  return false;

		NxRemoteDebugger* gRemoteDebugger;
		// Create the remote debugger
		gRemoteDebugger = m_PhysicsSDK->getFoundationSDK().getRemoteDebugger();

		/*
		gRemoteDebugger()->writeParameter(NxVec3(5,5,5), &gCameraObj, true, "Origin", NX_DBG_EVENTMASK_EVERYTHING);
		gRemoteDebugger()->writeParameter(NxVec3(0,0,0), &gCameraObj, true, "Target", NX_DBG_EVENTMASK_EVERYTHING);
		gRemoteDebugger()->writeParameter(NxVec3(0,1,0), &gCameraObj, true, "Up", NX_DBG_EVENTMASK_EVERYTHING);
		*/
		gRemoteDebugger->connect("localhost");


		NxVec3			m_DefaultGravity;
		m_DefaultGravity.set(0,0,0);
		m_sceneDesc.gravity = m_DefaultGravity;

		// Make Ageia run in this thread (and not create its own).
		//m_sceneDesc.flags &= ~NX_SF_SIMULATE_SEPARATE_THREAD;

		// Detect Ageia hardware. If none present then use software mode.
		if (m_PhysicsSDK->getHWVersion() != NX_HW_VERSION_NONE)
			m_sceneDesc.simType			= NX_SIMULATION_HW;
		else
			m_sceneDesc.simType			= NX_SIMULATION_SW;

		    // Set the physics parameters
		m_PhysicsSDK->setParameter(NX_SKIN_WIDTH, 0.01f);

			// Set the debug visualization parameters
		m_PhysicsSDK->setParameter(NX_VISUALIZATION_SCALE, DISPLAY_PHYSICS_BODIES);
		m_PhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES, DISPLAY_PHYSICS_BODIES );
		m_PhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES, DISPLAY_PHYSICS_BODIES);
		m_PhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_FNORMALS, DISPLAY_PHYSICS_BODIES);

//		NxActor* groundPlane;
//		groundPlane = CreateGroundPlane();


		return true;
	}


	

	// This is the threads equivalent of "main()".
	int PhysicsEngine::svc()
	{		
		m_bQuit = false;
		m_bPause = true;
		m_elapsedtime = 0;

		LOGDEBUG("begin PHYSICS loop");
		
		while(!m_bQuit)
		{
			if (m_bPause)
				ACE_OS::sleep(1);
			else
			{
				m_timer.stop();
				m_elapsedtime+= m_timer.elapsedTime();
				m_timer.start();
				
				if (m_elapsedtime >= m_updatetime)
				{		
					updateWorld(m_updatetime);
					m_elapsedtime = 0;
				}	
			}
		} 

		cleanup();

		LOGDEBUG("EXITING PHYSICS LOOP");

		return 0;
	}

	void PhysicsEngine::quit()
	{
		ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);
		m_bQuit = true;
	}

	void PhysicsEngine::cleanup()
	{
		ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);
		
		NxRemoteDebugger* gRemoteDebugger;			
		gRemoteDebugger = m_PhysicsSDK->getFoundationSDK().getRemoteDebugger();
		gRemoteDebugger->disconnect();

		if (m_PhysicsSDK)  m_PhysicsSDK->release();
	}

	void PhysicsEngine::updateWorld(float p_steptime)
	{
		map<long long, boost::shared_ptr<PhysicsLocale>>::const_iterator it;
		PhysicsLocale *templocale;

		it = m_physicsLocales.begin();
		while(it != m_physicsLocales.end())
		{
			templocale = it++->second.get();
			templocale->update(p_steptime);
		}	
	}

	long long PhysicsEngine::createLocale()
	{
		ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);

		// Add new object to map, and increment counter.
		boost::shared_ptr<PhysicsLocale> newobject(new PhysicsLocale());
		m_physicsLocales[m_nextLocaleIdAvailable] = newobject;
		return m_nextLocaleIdAvailable++;
	}
	
	void PhysicsEngine::destroyLocale(long long p_localeid)
	{
		ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);
		PhysicsLocale* obj = m_physicsLocales[p_localeid].get();

		if (obj != NULL)
		{
			m_physicsLocales.erase(p_localeid);
		}		

	}

	long long PhysicsEngine::createObject()
	{
		// Add new object to map, and increment counter.
		boost::shared_ptr<PhysicsObject> newobject(new PhysicsObject());
		m_physicsObjects[m_nextObjectIdAvailable] = newobject;
		return m_nextObjectIdAvailable++;

	}

	void PhysicsEngine::destroyObject(long long p_objectid)
	{
		PhysicsObject* obj = m_physicsObjects[p_objectid].get();

		if (obj != NULL)
		{
			if (obj->m_actor != NULL)
			{
				obj->m_actor->userData = (void*) -1;
			}

			m_physicsObjects.erase(p_objectid);
		}
	}

	PhysicsObject* PhysicsEngine::getObject(long long p_objectid)
	{
		map<long long, boost::shared_ptr<PhysicsObject>>::const_iterator it;

		it = m_physicsObjects.find(p_objectid);
		if (it != m_physicsObjects.end())
		{
			ERROR_ASSERT(NULL != it->second);
			return  it->second.get();
		}
		else
		{
			return NULL;
		}
	}

	PhysicsLocale* PhysicsEngine::getLocale(long long p_localeid)
	{
		map<long long, boost::shared_ptr<PhysicsLocale>>::const_iterator it;

		it = m_physicsLocales.find(p_localeid);
		if (it != m_physicsLocales.end())
		{
			ERROR_ASSERT(NULL != it->second);
			return  it->second.get();
		}
		else
		{
			return NULL;
		}
	}

};
